import Joint from './joint';
import CONSTANT from "../../constant";
import BasisTracker from "./basis-tracker";
import Vec3 from "../../common/vec3";
import Method from "../../common/method";

export default class CylindricalJoint extends Joint{
    constructor(config) {
        super(config, CONSTANT.JOINT_TYPE_CYLINDRICAL);
        Method.copyElements(config.localAxis1.elements,this.localBasis1,0,0,3);
        Method.copyElements(config.localAxis2.elements,this.localBasis2,0,0,3);
        this.buildLocalBasesFromX();
        this.angularError=new Float64Array(3);
        this.linearError=new Float64Array(3);
        this._basis = new BasisTracker(this);
        this._translSd = config.translationalSpringDamper.clone();
        this._translLm = config.translationalLimitMotor.clone();
        this._rotSd = config.rotationalSpringDamper.clone();
        this._rotLm = config.rotationalLimitMotor.clone();
        this._tM=new Float64Array(9);
    }

    //region
    get angle(){return this.angularError[0];};
    get angularErrorY(){return this.angularError[1];};
    get angularErrorZ(){return this.angularError[2];};
    set angle(n){this.angularError[0]=n;};
    set angularErrorY(n){this.angularError[1]=n;};
    set angularErrorZ(n){this.angularError[2]=n;};

    get translation(){return this.linearError[0];};
    get linearErrorY(){return this.linearError[1];};
    get linearErrorZ(){return this.linearError[2];};
    set translation(n){this.linearError[0]=n;};
    set linearErrorY(n){this.linearError[1]=n;};
    set linearErrorZ(n){this.linearError[2]=n;};
    //endregion

    getInfo(info,timeStep,isPositionPart) {
        let erp = this.getErp(timeStep,isPositionPart);
        let ae=this.angularError,le=this.linearError;
        let linRhsY = le[1] * erp;
        let linRhsZ = le[2] * erp;
        let angRhsY = ae[1] * erp;
        let angRhsZ = ae[2] * erp;
        let j;
        let translationalMotorMass = 1 / (this._b1._invMass + this._b2._invMass);
        let _basis=this._basis.elements;
        let rotationalMotorMass = this.computeEffectiveInertiaMoment(_basis[0],_basis[1],_basis[2]);
        if(this._translSd.frequency <= 0 || !isPositionPart) {
            let impulse = this._impulses[0];
            let row = info.rows[info.numRows++];
            this._resetRow(row,impulse);
            this.setSolverInfoRowLinear(row,le[0],this._translLm,translationalMotorMass,this._translSd,timeStep,isPositionPart);
            Method.setJacobian(_basis[0],_basis[1],_basis[2],this.relativeAnchor1,this.relativeAnchor2,row.jacobian.elements);
        }
        let impulse = this._impulses[1];
        let row = info.rows[info.numRows++];
        this._resetRow(row,impulse);
        row.rhs = linRhsY;
        row.cfm = 0;
        row.minImpulse = -1e65536;
        row.maxImpulse = 1e65536;
        Method.setJacobian(_basis[3],_basis[4],_basis[5],this.relativeAnchor1,this.relativeAnchor2,row.jacobian.elements);
        let impulse1 = this._impulses[2];
        let row1 = info.rows[info.numRows++];
        this._resetRow(row1,impulse1);
        row1.rhs = linRhsZ;
        row1.cfm = 0;
        row1.minImpulse = -1e65536;
        row1.maxImpulse = 1e65536;
        Method.setJacobian(_basis[6],_basis[7],_basis[8],this.relativeAnchor1,this.relativeAnchor2,row1.jacobian.elements);
        if(this._rotSd.frequency <= 0 || !isPositionPart) {
            let impulse = this._impulses[3];
            let row = info.rows[info.numRows++];
            this._resetRow(row,impulse);
            this.setSolverInfoRowAngular(row,ae[0],this._rotLm,rotationalMotorMass,this._rotSd,timeStep,isPositionPart);
            j = row.jacobian.elements;
            j[6] = _basis[0];j[7] = _basis[1];j[8] = _basis[2];
            j[9] = _basis[0];j[10] = _basis[1];j[11] = _basis[2];
        }
        let impulse2 = this._impulses[4];
        let row2 = info.rows[info.numRows++];
        this._resetRow(row2,impulse2);
        row2.rhs = angRhsY;
        row2.cfm = 0;
        row2.minImpulse = -1e65536;
        row2.maxImpulse = 1e65536;
        j = row2.jacobian.elements;
        j[6] = _basis[3];j[7] = _basis[4];j[8] = _basis[5];
        j[9] = _basis[3];j[10] = _basis[4];j[11] = _basis[5];
        let impulse3 = this._impulses[5];
        let row3 = info.rows[info.numRows++];
        this._resetRow(row3,impulse3);
        row3.rhs = angRhsZ;
        row3.cfm = 0;
        row3.minImpulse = -1e65536;
        row3.maxImpulse = 1e65536;
        j = row3.jacobian.elements;
        j[6] = _basis[6];j[7] = _basis[7];j[8] = _basis[8];
        j[9] = _basis[6];j[10] = _basis[7];j[11] = _basis[8];
    }
    _syncAnchors() {
        super._syncAnchors();
        let bt = this._basis;
        let m=this._tM,tv=this._tv;
        let bjb1=bt.joint.basis1,bjb2=bt.joint.basis2;
        let invM1 = bt.joint._b1._invMass;
        let invM2 = bt.joint._b2._invMass;
        let ae=this.angularError,le=this.linearError;
        let qX, qY, qZ, qW;
        let slerpQX, slerpQY, slerpQZ, slerpQW;
        let d = bjb1[0] * bjb2[0] + bjb1[1] * bjb2[1] + bjb1[2] * bjb2[2];
        if(d < -0.999999999) {
            Method.vecToQuat(bjb1[0],bjb1[1],bjb1[2],m);
            qX = m[0];qY = m[1];qZ = m[2];qW = 0;
        } else {
            Method.crossVectors(bjb1[0],bjb1[1],bjb1[2],bjb2[0],bjb2[1],bjb2[2],m);
            let w = Math.sqrt((1 + d) * 0.5);
            d = 0.5 / w;
            m[0] *= d;m[1] *= d;m[2] *= d;
            qX = m[0];qY = m[1];qZ = m[2];qW = w;
        }
        let q1X = 0,q1Y = 0,q1Z = 0,q1W = 1;
        let q2X = qX,q2Y = qY,q2Z = qZ,q2W = qW;
        let d1 = q1X * q2X + q1Y * q2Y + q1Z * q2Z + q1W * q2W;
        if(d1 < 0) {
            d1 = -d1;
            q2X = -q2X;q2Y = -q2Y;q2Z = -q2Z;q2W = -q2W;
        }
        if(d1 > 0.999999) {
            let dqX = q2X - q1X,dqY = q2Y - q1Y,dqZ = q2Z - q1Z,dqW = q2W - q1W;
            q2X = q1X + dqX * (invM1 / (invM1 + invM2));
            q2Y = q1Y + dqY * (invM1 / (invM1 + invM2));
            q2Z = q1Z + dqZ * (invM1 / (invM1 + invM2));
            q2W = q1W + dqW * (invM1 / (invM1 + invM2));
            let l = q2X * q2X + q2Y * q2Y + q2Z * q2Z + q2W * q2W;
            if(l > 1e-32) l = 1 / Math.sqrt(l);
            slerpQX = q2X * l;slerpQY = q2Y * l;slerpQZ = q2Z * l;slerpQW = q2W * l;
        } else {
            let theta = invM1 / (invM1 + invM2) * Math.acos(d1);
            q2X += q1X * -d1;q2Y += q1Y * -d1;q2Z += q1Z * -d1;q2W += q1W * -d1;
            let l = q2X * q2X + q2Y * q2Y + q2Z * q2Z + q2W * q2W;
            if(l > 1e-32) l = 1 / Math.sqrt(l);
            q2X *= l;q2Y *= l;q2Z *= l;q2W *= l;
            let sin = Math.sin(theta);
            let cos = Math.cos(theta);
            q1X *= cos;q1Y *= cos;q1Z *= cos;q1W *= cos;
            slerpQX = q1X + q2X * sin;slerpQY = q1Y + q2Y * sin;slerpQZ = q1Z + q2Z * sin;slerpQW = q1W + q2W * sin;
        }
        Method.quatToMat3(slerpQX,slerpQY,slerpQZ,slerpQW,m);
        Method.rotateVecTo(bjb1[0],bjb1[1],bjb1[2],m,m);
        let newXX = m[0], newXY = m[1], newXZ = m[2];
        let es=bt.elements;
        let prevXX = es[0],prevXY = es[1],prevXZ = es[2];
        let prevYX = es[3],prevYY = es[4],prevYZ = es[5];
        let d2 = prevXX * newXX + prevXY * newXY + prevXZ * newXZ;
        if(d2 < -0.999999999) {
            Method.vecToQuat(prevXX,prevXY,prevXZ,m);
            slerpQX = m[0];slerpQY = m[1];slerpQZ = m[2];slerpQW = 0;
        } else {
            let cX = prevXY * newXZ - prevXZ * newXY,cY = prevXZ * newXX - prevXX * newXZ,cZ = prevXX * newXY - prevXY * newXX;
            let w = Math.sqrt((1 + d2) * 0.5);
            d2 = 0.5 / w;
            cX *= d2;cY *= d2;cZ *= d2;
            slerpQX = cX;slerpQY = cY;slerpQZ = cZ;slerpQW = w;
        }
        Method.quatToMat3(slerpQX,slerpQY,slerpQZ,slerpQW,m);
        Method.rotateVecTo(prevYX,prevYY,prevYZ,m,m);
        let newYX = m[0], newYY = m[1], newYZ = m[2];
        let newZX = newXY * newYZ - newXZ * newYY, newZY = newXZ * newYX - newXX * newYZ, newZZ = newXX * newYY - newXY * newYX;
        if(newZX * newZX + newZY * newZY + newZZ * newZZ > 1e-6) {
            let l = newZX * newZX + newZY * newZY + newZZ * newZZ;
            if(l > 0) l = 1 / Math.sqrt(l);
            newZX *= l;newZY *= l;newZZ *= l;
        } else {
            Method.vecToQuat(newXX,newXY,newXZ,m);
            newZX=m[0];newZY=m[1];newZZ=m[2];
        }
        newYX = newZY * newXZ - newZZ * newXY;newYY = newZZ * newXX - newZX * newXZ;newYZ = newZX * newXY - newZY * newXX;
        Method.setM3X3(es,newXX,newXY,newXZ,newYX,newYY,newYZ,newXZ,newZY,newZZ);
        let bs1=this.basis1,bs2=this.basis2;
        Method.crossVectors(bs1[0],bs1[1],bs1[2],bs2[0],bs2[1],bs2[2],m);
        let angErrorX = m[0], angErrorY = m[1], angErrorZ = m[2];
        let cos = bs1[0] * bs2[0] + bs1[1] * bs2[1] + bs1[2] * bs2[2];
        let theta = cos <= -1 ? 3.14159265358979 : cos >= 1 ? 0 : Math.acos(cos);
        let l = angErrorX * angErrorX + angErrorY * angErrorY + angErrorZ * angErrorZ;
        if(l > 0) l = 1 / Math.sqrt(l);
        angErrorX *= l;angErrorY *= l;angErrorZ *= l;
        angErrorX *= theta;angErrorY *= theta;angErrorZ *= theta;
        ae[1] = angErrorX * es[3] + angErrorY * es[4] + angErrorZ * es[5];
        ae[2] = angErrorX * es[6] + angErrorY * es[7] + angErrorZ * es[8];
        Method.crossVectors(bs1[3],bs1[4],bs1[5],bs2[3],bs2[4],bs2[5],m);
        let perpCrossX = m[0], perpCrossY = m[1], perpCrossZ = m[2];
        cos = bs1[3] * bs2[3] + bs1[4] * bs2[4] + bs1[5] * bs2[5];
        ae[0] = cos <= -1 ? 3.14159265358979 : cos >= 1 ? 0 : Math.acos(cos);
        if(perpCrossX * es[0] + perpCrossY * es[1] + perpCrossZ * es[2] < 0) {
            ae[0] = -ae[0];
        }
        Method.subArray(this.anchor2,this.anchor1,tv,0,0,0,3);
        Method.rotateVecTo(tv[0],tv[1],tv[2],es,m);
        le[0] = m[0];
        le[1] = m[1];
        le[2] = m[2];
    }
    _getVelocitySolverInfo(timeStep,info) {
        super._getVelocitySolverInfo(timeStep,info);
        this.getInfo(info,timeStep,false);
    }
    _getPositionSolverInfo(info) {
        super._getPositionSolverInfo(info);
        this.getInfo(info,null,true);
    }
    getAxis1() {
        return new Vec3(this.basis1[0],this.basis1[1],this.basis1[2]);
    }
    getAxis2() {
        return new Vec3(this.basis2[0],this.basis2[1],this.basis2[2]);
    }
    getAxis1To(axis) {
        let es=axis.elements;
        es[0] = this.basis1[0];es[1] = this.basis1[1];es[2] = this.basis1[2];
    }
    getAxis2To(axis) {
        let es=axis.elements;
        es[0] = this.basis2[0];es[1] = this.basis2[1];es[2] = this.basis2[2];
    }
    getLocalAxis1() {
        return new Vec3(this.localBasis1[0],this.localBasis1[1],this.localBasis1[2]);
    }
    getLocalAxis2() {
        return new Vec3(this.localBasis2[0],this.localBasis2[1],this.localBasis2[2]);
    }
    getLocalAxis1To(axis) {
        let es=axis.elements;
        es[0] = this.localBasis1[0];es[1] = this.localBasis1[1];es[2] = this.localBasis1[2];
    }
    getLocalAxis2To(axis) {
        let es=axis.elements;
        es[0] = this.localBasis2[0];es[1] = this.localBasis2[1];es[2] = this.localBasis2[2];
    }
    getTranslationalSpringDamper() {
        return this._translSd;
    }
    getRotationalSpringDamper() {
        return this._rotSd;
    }
    getTranslationalLimitMotor() {
        return this._translLm;
    }
    getRotationalLimitMotor() {
        return this._rotLm;
    }
    getAngle() {
        return this.angularError[0];
    }
    getTranslation() {
        return this.linearError[0];
    }
}